#include "mousecc.h"
MouseCC::MouseCC( ){
    l=new Log("MouseCC");
    this->X=this->Y=this->angle_=0;
    this->feedback_ = false;
}
MouseCC::~MouseCC(){
    delete this->l;
}
void  MouseCC::run(){
    //ident 0 = GB
    //ident 1 = LT
    int ident0=0,ident1=1; //mousenumbers
    this->m0.setDPI(1);
    this->m1.setDPI(DPI);

    this->m0.start(&ident0);
    this->m1.start(&ident1);

    this->m0.setX(0);
    this->m0.setY(0);

    this->m1.setX(0);
    this->m1.setY(0);

    while(1){
        if(abs(m0.getX())>TRESH || abs(m1.getX())>TRESH || abs(m0.getY())>TRESH || abs(m1.getY())>TRESH){
        /*
         * lock both mice, read their data, reset their buffer, unlock and calculate
         */
            m0.lock();
            m1.lock();

            m0_dx = m0.getX();
            m0_dy = m0.getY();
            m0.reset();
            m0.unlock();

            m1_dx = m1.getX();
            m1_dy = m1.getY();
            m1.reset();
            m1.unlock();
            this->calculate();
            //pseudo code follows
            //Determine if we have deviated enough to call feedback
            if( this->feedback_ ){
            	if( abs(angleDifference_) >= devi_){
            		//pointer naar een functie doorgeven lijkt nog het gemakkelijkste te zijn
            		//this->pt2fb_(angleDifference_);
            	}else{
            		//angleDifference = ....
            	}
            }
        }
    }
}
void MouseCC::calculate(){
    //cout << "m1_dx: " << this->m1_dx << ", m1_dy: "<< this->m1_dy;
    //cout << ", m0_dx: " << this->m0_dx << ", m0_dy: "<< this->m0_dy << endl;

    //check distance between dx's
    /*if( abs(this->m0_dx - this->m1_dx) > 2*DIST)
        cout << "dx is groter dan dist"<< endl;
    if( abs(this->m0_dy - this->m1_dy) > 2*DIST)
        cout << "dy is groter dan dist"<< endl;
*/
    /*
     * Check the pdf 'dead_reckoning_with_mouse_sensors.pdf' for more information
     */
    //u-matrix berekenen
    double dx    = (this->m0_dx + this->m1_dx)/2.0;
    double dy    = (this->m0_dy + this->m1_dy)/2.0;
    double angle = (this->m0_dy - this->m1_dy)/(2.0*DIST); //eventueel zelf uitrekenen als niet klopt

    //globale X/Y coordinaten berekenen
    this->X += dx*cos(this->angle_) - dy*sin(this->angle_);
    this->Y += dx*sin(this->angle_) + dy*cos(this->angle_);
    this->angle_ += angle;

    //Afstand berekenen
    /*int dist;
    dist = sqrt((this->X*this->X)+((this->Y*this->Y)));
    */

    //cout << "dx: "<< dx <<", dy: "<<dy<<", angle: "<<angle<<endl;
    cout << "X: "<< this->X <<", Y: "<<this->Y<<", angle_: "<<(180/M_PI)*this->angle_ <<endl;
    //<< ", Distance to (0,0): "<<dist*(2.5/DIST)
}
void MouseCC::setupFeedback(float devi, void(*pt2fb)(float) ){
	this->feedback_ = true;
    this->pt2fb_ = pt2fb;
}
void MouseCC::stopFeedback(){
	this->feedback_ = false;
}
